//------------------------------------------------------------------------------
// Marbles Service
//
// Professional Microsoft Robotics Developer Studio
//
// Updated for RDS 2008 (V2.0)
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

using System;
using System.Collections.Generic;

using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using System.ComponentModel;

using xna = Microsoft.Xna.Framework;
using xnagrfx = Microsoft.Xna.Framework.Graphics;

namespace Robotics.ProMRDS.Marbles
{
    [DisplayName("ProMRDS Marbles")]
    [Description("ProMRDS Marbles Demo")]
    [Contract(Contract.Identifier)]
    public class Marbles : DsspServiceBase
    {
        const int MarbleRate = 500;     // ms between marble drops
        const int maxMarbleCount = 200; // maximum number of marbles at one time
        int _marbleCount = 0;           // total number of marbles created
        Random _random = new Random();  // random number generator
        Queue<VisualEntity> marbles = new Queue<VisualEntity>();    // keeps track of marbles

        [Partner("Engine", 
            Contract = engineproxy.Contract.Identifier, 
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        private engineproxy.SimulationEnginePort _engineStub = 
            new engineproxy.SimulationEnginePort();

        // Main service port
        [ServicePort("/Marbles", AllowMultipleInstances=false)]
        private MarblesOperations _mainPort = 
            new MarblesOperations();

        public Marbles(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        protected override void Start()
        {
            base.Start();
            // Orient sim camera view point
            SetupCamera();
            // Add objects (entities) in our simulated world
            PopulateWorld();
        }

        private void SetupCamera()
        {
            // Set up initial view
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(-2.72f, 4.66f, -3.51f);
            view.LookAtPoint = new Vector3(-2.14f, 4.18f, -2.84f);
            SimulationEngine.GlobalInstancePort.Update(view);
        }

        private void PopulateWorld()
        {
            // add some additional cameras
            CameraEntity camera2 = new CameraEntity();
            camera2.Location = new xna.Vector3(-2.65f, 0.07f, 1.42f);
            camera2.LookAt = new xna.Vector3(-1.98f, 0.37f, 0.74f);
            camera2.State.Name = "MarbleMill View";
            SimulationEngine.GlobalInstancePort.Insert(camera2);

            CameraEntity camera3 = new CameraEntity();
            camera3.Location = new xna.Vector3(0, 4.86f, 0);
            camera3.LookAt = new xna.Vector3(0, 3.86f, 0);
            camera3.State.Name = "View From Above";
            SimulationEngine.GlobalInstancePort.Insert(camera3);

            // add a SkyDome and Ground
            SkyDomeEntity sky = new SkyDomeEntity("skyDome.dds", "sky_diff.dds");
            SimulationEngine.GlobalInstancePort.Insert(sky);

            // Add a directional light to simulate the sun.
            LightSourceEntity sun = new LightSourceEntity();
            sun.State.Name = "Sun";
            sun.Type = LightSourceEntityType.Directional;
            sun.Color = new Vector4(0.8f, 0.8f, 0.8f, 1);
            sun.Direction = new Vector3(0.5f, -.75f, 0.5f);
            SimulationEngine.GlobalInstancePort.Insert(sun);

            // create a large horizontal plane, at zero elevation.
            HeightFieldEntity ground = new HeightFieldEntity(
                "simple ground", // name
                "HexTile.dds", // texture image
                new MaterialProperties("ground",
                    0.2f, // restitution
                    0.5f, // dynamic friction
                    0.5f) // static friction
                );
            SimulationEngine.GlobalInstancePort.Insert(ground);

            // Build an array of kinematic capsules
            CapsuleArray pegboard = new CapsuleArray(10, 10, 0.2f, 0.2f, new Vector3(0, 2.0f, 0));
            pegboard.State.Name = "Pegboard";
            SimulationEngine.GlobalInstancePort.Insert(pegboard);

            // Build a ramp to collect the marbles
            BoxShape[] rails = new BoxShape[2];
            BoxShape rail = new BoxShape(
                new BoxShapeProperties(
                  10, //kg
                  new Pose(),
                  new Vector3(2.5f, 0.06f, 0.2f)));
            rail.State.Name = "railshape";

            SingleShapeEntity ramp = new SingleShapeEntity(rail, new Vector3());
            ramp.State.Flags |= EntitySimulationModifiers.Kinematic;
            ramp.State.Name = "Ramp";
            ramp.State.Pose = new Pose(
                new Vector3(-0.025961088f, 1.5949626f, 0),
                UIMath.EulerToQuaternion(new xna.Vector3(0, 0, 11.06f)));
            ramp.State.Assets.DefaultTexture = "Ground2.bmp";
            SimulationEngine.GlobalInstancePort.Insert(ramp);

            Base millBase = new Base(0.25f, 0.7f, 0.16f, new Vector3(-1.1f,0,0));
            millBase.State.Name = "base";
            SimulationEngine.GlobalInstancePort.Insert(millBase);

            Blades blades = new Blades(0.09f, 0.4f, 0.15f, new Vector3(-1.1f, 0.76f, 0));
            blades.State.Name = "blades";
            SimulationEngine.GlobalInstancePort.Insert(blades);

            CreateMarbles();
            Activate(Arbiter.Receive(false, TimeoutPort(MarbleRate), DropAMarble));
        }

        void DropAMarble(DateTime time)
        {
            MarbleEntity marble = (MarbleEntity)marbles.Dequeue();
            marble.CurrentMarbleMode = MarbleEntity.MarbleMode.Position;
            marbles.Enqueue(marble);
            Activate(Arbiter.Receive(false, TimeoutPort(MarbleRate), DropAMarble));
        }

        void CreateMarbles()
        {
            Vector3 holdingPosition = new Vector3(1000, 1000, 1000);
            for (_marbleCount = 0; _marbleCount <= maxMarbleCount; _marbleCount++)
            {
                MarbleEntity marble = new MarbleEntity(0.07f, holdingPosition);
                marble.State.Name = "Marble" + _marbleCount++;
                marble.State.Flags = EntitySimulationModifiers.Kinematic;

                // Insert entity in simulation.  
                SimulationEngine.GlobalInstancePort.Insert(marble);
                marbles.Enqueue(marble);
            }
        }
    }

    public static class Contract
    {
        public const string Identifier = "http://www.promrds.com/contracts/2007/06/marbles.html";
    }

    public class MarblesOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop>
    {
    }

    [DataContract]
    [EditorAttribute(typeof(GenericObjectEditor), typeof(System.Drawing.Design.UITypeEditor))]
    public class CapsuleArray : VisualEntity
    {
        int _rows;
        int _columns;
        float _columnSpacing;
        float _rowSpacing;

        [DataMember]
        public int Rows
        {
            get { return _rows; }
            set { _rows = value; }
        }

        [DataMember]
        public int Columns
        {
            get { return _columns; }
            set { _columns = value; }
        }

        [DataMember]
        public float ColumnSpacing
        {
            get { return _columnSpacing; }
            set { _columnSpacing = value; }
        }

        [DataMember]
        public float RowSpacing
        {
            get { return _rowSpacing; }
            set { _rowSpacing = value; }
        }

        /// <summary>
        /// Default constructor
        /// </summary>
        public CapsuleArray() 
        { 
        }

        /// <summary>
        /// Initial (non deserialized) constructor
        /// </summary>
        public CapsuleArray(int Rows, int Columns, float RowSpacing, float ColumnSpacing, Vector3 position)
        {
            _rows = Rows;
            _columns = Columns;
            _rowSpacing = RowSpacing;
            _columnSpacing = ColumnSpacing;

            State.Pose.Position = position;
            State.Flags |= EntitySimulationModifiers.Kinematic;
        }

        /// <summary>
        /// Initialization
        /// </summary>
        /// <param name="device"></param>
        /// <param name="physicsEngine"></param>
        public override void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            try
            {
                float halfOffset = _columnSpacing / 2.0f;
                float pegRadius = _columnSpacing * 0.1f;
                float pegHeight = _columnSpacing * 1.2f;

                InitError = string.Empty;
                Pose cPose = new Pose(new Vector3(0, 0, 0), UIMath.EulerToQuaternion(new xna.Vector3(90, 0, 0)));
                float xOffset = (_columns - 1) * _columnSpacing * 0.5f;
                for (int r = 0; r < _rows; r++)
                {
                    cPose.Position.Y = r * _rowSpacing + pegRadius;
                    for (int c = 0; c < _columns; c++)
                    {
                        cPose.Position.X = c * _columnSpacing - xOffset;
                        if ((r & 1) != 0)
                            cPose.Position.X += halfOffset;

                        CapsuleShape peg = new CapsuleShape(
                            new CapsuleShapeProperties(
                                1,
                                new Pose(cPose.Position, cPose.Orientation),
                                pegRadius,
                                pegHeight));
                        peg.State.Material = new MaterialProperties("bouncy", 0.9f, 0.2f, 0.5f);

                        peg.State.Name = "Peg_" + r.ToString() + "_" + c.ToString();
                        base.State.PhysicsPrimitives.Add(peg);
                    }
                }

                // To be visible to the physics simulation we have to explicitly insert ourselves
                CreateAndInsertPhysicsEntity(physicsEngine);

                // call base initialize so it can load effect, cache handles, compute
                // bounding geometry
                base.Initialize(device, physicsEngine);

            }
            catch (Exception ex)
            {
                HasBeenInitialized = false;
                InitError = ex.ToString();
            }
        }
    }

    [DataContract]
    [EditorAttribute(typeof(GenericObjectEditor), typeof(System.Drawing.Design.UITypeEditor))]
    public class Base : VisualEntity
    {
        float _width;
        float _height;
        float _depth;

        [DataMember]
        public float Width
        {
            get { return _width; }
            set { _width = value; }
        }

        [DataMember]
        public float Height
        {
            get { return _height; }
            set { _height = value; }
        }

        [DataMember]
        public float Depth
        {
            get { return _depth; }
            set { _depth = value; }
        }

        /// <summary>
        /// Default constructor
        /// </summary>
        public Base()
        {
        }

        /// <summary>
        /// Initial (non deserialized) constructor
        /// </summary>
        public Base(float width, float height, float depth, Vector3 position)
        {
            _width = width;
            _height = height;
            _depth = depth;

            State.Pose.Position = position;
            State.Flags |= EntitySimulationModifiers.Kinematic;
        }

        /// <summary>
        /// Initialization
        /// </summary>
        /// <param name="device"></param>
        /// <param name="physicsEngine"></param>
        public override void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            try
            {
                float supportDepth = _depth * 0.2f;

                // create two supports
                BoxShape support1 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(new Vector3(0, _height/2.0f, -Depth / 2.0f - supportDepth / 2.0f)),
                        new Vector3(_width, _height, supportDepth)));
                support1.State.Name = "support1";
                support1.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(support1);

                BoxShape support2 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(new Vector3(0, _height/2.0f,  Depth / 2.0f + supportDepth / 2.0f)),
                        new Vector3(_width, _height, supportDepth)));
                support2.State.Name = "support2";
                support2.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(support2);

                float crossWidth = 0.9f * _width / 3.0f;

                // create four crossmembers
                BoxShape cross1 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(new Vector3(-(_width - crossWidth)/2.0f, _height + crossWidth/2.0f, -(_depth + supportDepth)/2.0f)),
                        new Vector3(crossWidth, crossWidth, supportDepth)));
                cross1.State.Name = "cross1";
                cross1.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(cross1);

                BoxShape cross2 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(new Vector3((_width - crossWidth) / 2.0f, _height + crossWidth / 2.0f, -(_depth + supportDepth) / 2.0f)),
                        new Vector3(crossWidth, crossWidth, supportDepth)));
                cross2.State.Name = "cross2";
                cross2.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(cross2);

                BoxShape cross3 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(new Vector3(-(_width - crossWidth) / 2.0f, _height + crossWidth / 2.0f, (_depth + supportDepth)/2.0f)),
                        new Vector3(crossWidth, crossWidth, supportDepth)));
                cross3.State.Name = "cross3";
                cross3.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(cross3);

                BoxShape cross4 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(new Vector3((_width - crossWidth) / 2.0f, _height + crossWidth / 2.0f, (_depth + supportDepth)/2.0f)),
                        new Vector3(crossWidth, crossWidth, supportDepth)));
                cross4.State.Name = "cross4";
                cross4.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(cross4);


                // To be visible to the physics simulation we have to explicitly insert ourselves
                CreateAndInsertPhysicsEntity(physicsEngine);

                base.State.Assets.DefaultTexture = "MayangConcrP.dds";
                // call base initialize so it can load effect, cache handles, compute
                // bounding geometry
                base.Initialize(device, physicsEngine);

            }
            catch (Exception ex)
            {
                HasBeenInitialized = false;
                InitError = ex.ToString();
            }
        }
    }

    [DataContract]
    [EditorAttribute(typeof(GenericObjectEditor), typeof(System.Drawing.Design.UITypeEditor))]
    public class Blades : VisualEntity
    {
        float _axleWidth;
        float _bladeLength;
        float _depth;

        [DataMember]
        public float Depth
        {
            get { return _depth; }
            set { _depth = value; }
        }

        [DataMember]
        public float AxleWidth
        {
            get { return _axleWidth; }
            set { _axleWidth = value; }
        }

        [DataMember]
        public float BladeLength
        {
            get { return _bladeLength; }
            set { _bladeLength = value; }
        }

        /// <summary>
        /// Default constructor
        /// </summary>
        public Blades()
        {
        }

        /// <summary>
        /// Initial (non deserialized) constructor
        /// </summary>
        public Blades(float axleWidth, float bladeLength, float depth, Vector3 position)
        {
            _axleWidth = axleWidth;
            _bladeLength = bladeLength;
            _depth = depth;

            State.Pose.Position = position;
        }

        /// <summary>
        /// Initialization
        /// </summary>
        /// <param name="device"></param>
        /// <param name="physicsEngine"></param>
        public override void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            try
            {
                // create the Axle
                Pose cPose = new Pose(new Vector3(0, 0, 0), UIMath.EulerToQuaternion(new xna.Vector3(90, 0, 0)));
                CapsuleShape axle = new CapsuleShape(
                    new CapsuleShapeProperties(
                        1,
                        new Pose(cPose.Position, cPose.Orientation),
                        _axleWidth/2.0f,
                        _depth * 1.5f));
                axle.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                axle.State.Name = "Axle";
                base.State.PhysicsPrimitives.Add(axle);

                // add the blades
                BoxShape blade1 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(),
                        new Vector3(_bladeLength * 2.0f, _bladeLength * 0.1f, _depth)));
                blade1.State.Name = "blade1";
                blade1.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(blade1);

                BoxShape blade2 = new BoxShape(
                    new BoxShapeProperties(
                        1, // kg
                        new Pose(),
                        new Vector3(_bladeLength * 0.1f, _bladeLength * 2.0f, _depth)));
                blade2.State.Name = "blade2";
                blade2.State.Material = new MaterialProperties("slippery", 0.1f, 0.01f, 0.01f);
                base.State.PhysicsPrimitives.Add(blade2);

                // To be visible to the physics simulation we have to explicitly insert ourselves
                CreateAndInsertPhysicsEntity(physicsEngine);

                base.State.Assets.DefaultTexture = "Wood_cherry.jpg";

                // call base initialize so it can load effect, cache handles, compute
                // bounding geometry
                base.Initialize(device, physicsEngine);

            }
            catch (Exception ex)
            {
                HasBeenInitialized = false;
                InitError = ex.ToString();
            }
        }
    }

    [DataContract]
    [EditorAttribute(typeof(GenericObjectEditor), typeof(System.Drawing.Design.UITypeEditor))]
    public class MarbleEntity : SingleShapeEntity
    {
        float _radius;
        static Random _random = new Random();

        [DataMember]
        public float Radius
        {
            get { return _radius; }
            set { _radius = value; }
        }

        [DataContract]
        public enum MarbleMode
        {
            Holding,
            Position,
            ReadyToDrop,
            Active
        }

        MarbleMode _marbleMode;

        [DataMember]
        public MarbleMode CurrentMarbleMode
        {
            get { return _marbleMode; }
            set { _marbleMode = value; }
        }

        public MarbleEntity()
        {
        }

        public MarbleEntity(float radius, Vector3 position)
        {
            _radius = radius;
            base.State.Pose = new Pose(position);
        }

        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            SphereShape sphere = new SphereShape(
                new SphereShapeProperties(
                    1, // kg
                    new Pose(), // default pose
                    0.070f));
            sphere.SphereState.DiffuseColor.X = (float)_random.NextDouble();
            sphere.SphereState.DiffuseColor.Y = (float)_random.NextDouble();
            sphere.SphereState.DiffuseColor.Z = (float)_random.NextDouble();
            sphere.SphereState.DiffuseColor.W = 1.0f;
            
            // prevent the balls from rolling forever on the floor
            this.State.MassDensity.LinearDamping = 0.05f;

            base.SphereShape = sphere;

            base.Initialize(device, physicsEngine);

            base.PhysicsEntity.SolverIterationCount = 128;
        }

        public override void Update(FrameUpdate update)
        {
            switch (_marbleMode)
            {
                case MarbleMode.Position:
                    PhysicsEntity.IsKinematic = true;
                    State.Pose.Position = new Vector3((float)(_random.NextDouble() * 0.075 - 0.075), 4.2f, 0);
                    if (PhysicsEntity != null)
                        PhysicsEntity.SetPose(State.Pose);

                    _marbleMode = MarbleMode.ReadyToDrop;
                    break;
                
                case MarbleMode.ReadyToDrop:
                    if (PhysicsEntity != null)
                        PhysicsEntity.IsKinematic = false;

                    _marbleMode = MarbleMode.Active;
                    break;
            }

            base.Update(update);
        }
    }
}
